//
// Created by fazhehy on 2024/6/9.
//

#include "flash.h"

static FLASH_EraseInitTypeDef EraseInitStruct = {
        .TypeErase = FLASH_TYPEERASE_PAGES,       //页擦除
        .Banks = FLASH_BANK_1,
        .PageAddress = FLASH_BANK1_END,
        .NbPages = 1
};

uint32_t addr = FLASH_BANK1_END - 0x0000003ff;

flash_data_union flash_union_buffer[255];

void flash_write_page_from_buffer(uint32_t sector_num, uint32_t page_num, uint32_t len)
{
    uint32_t PageError = 0;
    HAL_FLASH_Unlock();
    __disable_irq();                             //擦除前关闭中断
    if (HAL_FLASHEx_Erase(&EraseInitStruct, &PageError) == HAL_OK){
    }
    else{
        while(1);
    }
    __enable_irq();

    for (int i = 0; i < len; ++i) {
        if(HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,addr, flash_union_buffer[i].int32_type)==HAL_OK){

        }
        else{
            while (1);
        }
        addr += 4;
    }
    addr = FLASH_BANK1_END - 0x0000003ff;
}

void flash_read_page_to_buffer(uint32_t sector_num, uint32_t page_num, uint32_t len)
{
    uint32_t * flash_addr = (uint32_t *)(FLASH_BANK1_END - 0x0000003ff);
    for (int i = 0; i < len; ++i) {
        flash_union_buffer[i].uint32_type = flash_addr[i];
    }
}

void flash_write_parameter()
{
    uint16_t cnt = 1;
    flash_union_buffer[cnt ++].float_type = mx_offset;
    flash_union_buffer[cnt ++].float_type = my_offset;
    flash_union_buffer[cnt ++].float_type = mz_offset;
    flash_union_buffer[cnt ++].float_type = mx_k;
    flash_union_buffer[cnt ++].float_type = my_k;
    flash_union_buffer[cnt ++].float_type = mz_k;

    flash_union_buffer[cnt ++].float_type = mag_angle;

    flash_union_buffer[cnt ++].int16_type = outdoor_kun_gnss.speed;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_gnss.p;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_gnss.d;
    flash_union_buffer[cnt ++].int16_type = outdoor_kun_sound.speed;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_sound.p;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_sound.d;

    flash_union_buffer[cnt ++].int16_type = outdoor_precise_gnss.speed;
    flash_union_buffer[cnt ++].float_type = outdoor_precise_gnss.p;
    flash_union_buffer[cnt ++].float_type = outdoor_precise_gnss.d;
    flash_union_buffer[cnt ++].int16_type = outdoor_kun_sound.speed;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_sound.p;
    flash_union_buffer[cnt ++].float_type = outdoor_kun_sound.d;

    flash_union_buffer[cnt ++].int16_type = indoor_kun_sound.speed;
    flash_union_buffer[cnt ++].float_type = indoor_kun_sound.p;
    flash_union_buffer[cnt ++].float_type = indoor_kun_sound.d;

    flash_union_buffer[cnt ++].int16_type = indoor_precise_sound.speed;
    flash_union_buffer[cnt ++].float_type = indoor_precise_sound.p;
    flash_union_buffer[cnt ++].float_type = indoor_precise_sound.d;

    flash_union_buffer[0].uint8_type = cnt;

    flash_write_page_from_buffer(0, 0, cnt);
}

void flash_read_parameter()
{
    uint16_t cnt = 0;
    flash_read_page_to_buffer(0, 0, 1);
    cnt = flash_union_buffer[0].uint8_type;
    flash_read_page_to_buffer(0, 0, cnt);
    cnt = 1;

    mx_offset = flash_union_buffer[cnt ++].float_type;
    my_offset = flash_union_buffer[cnt ++].float_type;
    mz_offset = flash_union_buffer[cnt ++].float_type;
    mx_k = flash_union_buffer[cnt ++].float_type;
    my_k = flash_union_buffer[cnt ++].float_type;
    mz_k = flash_union_buffer[cnt ++].float_type;

    mag_angle = flash_union_buffer[cnt ++].float_type;

    outdoor_kun_gnss.speed = flash_union_buffer[cnt ++].int16_type;
    outdoor_kun_gnss.p = flash_union_buffer[cnt ++].float_type;
    outdoor_kun_gnss.d = flash_union_buffer[cnt ++].float_type;
    outdoor_kun_sound.speed = flash_union_buffer[cnt ++].int16_type;
    outdoor_kun_sound.p = flash_union_buffer[cnt ++].float_type;
    outdoor_kun_sound.d = flash_union_buffer[cnt ++].float_type;

    outdoor_precise_gnss.speed = flash_union_buffer[cnt ++].int16_type;
    outdoor_precise_gnss.p = flash_union_buffer[cnt ++].float_type;
    outdoor_precise_gnss.d = flash_union_buffer[cnt ++].float_type;
    outdoor_precise_sound.speed = flash_union_buffer[cnt ++].int16_type;
    outdoor_precise_sound.p = flash_union_buffer[cnt ++].float_type;
    outdoor_precise_sound.d = flash_union_buffer[cnt ++].float_type;

    indoor_kun_sound.speed = flash_union_buffer[cnt ++].int16_type;
    indoor_kun_sound.p = flash_union_buffer[cnt ++].float_type;
    indoor_kun_sound.d = flash_union_buffer[cnt ++].float_type;

    indoor_precise_sound.speed = flash_union_buffer[cnt ++].int16_type;
    indoor_precise_sound.p = flash_union_buffer[cnt ++].float_type;
    indoor_precise_sound.d = flash_union_buffer[cnt ++].float_type;
}

void flash_write_default()
{
    mx_offset = 0.167f;
    my_offset = -0.079f;
    mz_offset = 0.419f;
    mx_k = 0.489f;
    my_k = 0.505f;
    mz_k = 0.495f;

    mag_angle = 270;

    outdoor_kun_gnss.speed = 3000;
    outdoor_kun_gnss.p = 10;
    outdoor_kun_gnss.d = 7.5f;
    outdoor_kun_sound.speed = 1500;
    outdoor_kun_sound.p = 2.5f;
    outdoor_kun_sound.d = 1.5f;

    outdoor_precise_gnss.speed = 5000;
    outdoor_precise_gnss.p = 3;
    outdoor_precise_gnss.d = 1.8f;
    outdoor_precise_sound.speed = 1800;
    outdoor_precise_sound.p = 8;
    outdoor_precise_sound.d = 5;

    indoor_kun_sound.speed = 1500;
    indoor_kun_sound.p = 2.5f;
    indoor_kun_sound.d = 1.5f;

    indoor_precise_sound.speed = 1500;
    indoor_precise_sound.p = 2.5f;
    indoor_precise_sound.d = 1.5f;
}
